#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Gyro,           sensorGyro)
#pragma config(Sensor, in2,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    FlipperPot,     sensorPotentiometer)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_4,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_5,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_6,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           RightBaseBack, tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port2,           LeftBaseFront, tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_4, 1000)
#pragma config(Motor,  port3,           RightArm,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port4,           LeftArm,       tmotorVex393, openLoop)
#pragma config(Motor,  port5,           TheJaw,        tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port6,           FlipperMotorLeft, tmotorVex269, openLoop, reversed, encoder, encoderPort, I2C_5, 1000)
#pragma config(Motor,  port7,           FlipperMotorRight, tmotorVex269, openLoop, encoder, encoderPort, I2C_6, 1000)
#pragma config(Motor,  port8,           LeftBaseBack,  tmotorVex393, openLoop, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port9,           turntablemotor, tmotorVex269, openLoop)
#pragma config(Motor,  port10,          RightBaseFront, tmotorVex393HighSpeed, PIDControl, encoder, encoderPort, I2C_3, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!


#define SpeedSlow 60
#define SpeedModerate 75
#define SpeedFast 127

////////////////////////////////////////////////////
//			FLIPPER POTENTIOMETER SETTINGS
////////////////////////////////////////////////////
#define FlipperToFloor      2560
#define FlipperScore       	830
#define FlipperUp45Degrees  3640
#define FlipperScoreHigh   	2100

////////////////////////////////////////////////////
//			POTENTIOMETER ARM SETTINGS
////////////////////////////////////////////////////
#define ArmsFloor 1060
#define ArmsTrough 2222//2260
#define ArmsHigh 2780
// ARMS HOLD VALUE
#define ArmsMotorHold 10


////////////////////////////////////////////////////
//			Programming Skills Functions
////////////////////////////////////////////////////

void flippergodown()
{
	while (SensorValue [FlipperPot] > FlipperToFloor)
	{
		motor(FlipperMotorLeft) = -63;
		motor(FlipperMotorRight) = -63;
		motor(TheJaw) = 127;
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
	motor(TheJaw) = 10;
}

void flippertotrough()
{
	// first reead the pot on flipper into a local variable
	// keep movimg the flliper down until it reaCHES  the "trough" pot setting
	// once it reaches the trough setting - quit moving the flipper (open jaws?)
	while (SensorValue[FlipperPot] > FlipperScore)
	{
		motor(FlipperMotorRight) = -83;
		motor(FlipperMotorLeft) = -83;
		if (SensorValue[FlipperPot] < FlipperToFloor)
		{
			motor(TheJaw) = 127;
		}
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
	motor(TheJaw) = 0;
}

void flipperup()
{
	//jaws will be open
	while (SensorValue [FlipperPot] < FlipperUp45Degrees)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
		if (SensorValue [FlipperPot] > FlipperUp45Degrees)
		{
			//after 45 degrees close jaws
			break;
		}
	}
	wait1Msec(1000);
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
	motor(TheJaw) = -127;
	wait1Msec(500);
	motor(TheJaw) = -10;
}

void flipperhigh()
{
	motor(TheJaw) = -127;
	while (SensorValue [FlipperPot] < FlipperScoreHigh)
	{
		motor(FlipperMotorLeft) = 83;
		motor(FlipperMotorRight) = 83;
	}
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
	motor(TheJaw) = 0;
}

void flipperscorehigh()
{
	while (SensorValue[FlipperPot] > FlipperScoreHigh)
	{
		motor(FlipperMotorRight) = -60; //83
		motor(FlipperMotorLeft) = -60; //83
	}

	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
	motor(TheJaw) = 70;
}

void ClearEncoders()
{
	nMotorEncoder[RightBaseBack] = 0;
	nMotorEncoder[RightBaseFront] = 0;
	nMotorEncoder[LeftBaseBack] = 0;
	nMotorEncoder[LeftBaseFront] = 0;
}


void WaitForLCD()
{

	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
}

void RampUp(int iSpeed, int iDistance)
{
	// Ramp up as we start (the first 1/2 second - we should always move at least 1/2 second)
	int pwr = 1;
	for(int t = 1; t < iSpeed; t += 1)
	{
		// increment the power by t each time through the loop - starts at 50 and builds
		pwr = pwr + t;
		//set motors to pwr
		int iLE = abs(nMotorEncoder(LeftBaseBack));
		motor(LeftBaseBack) = pwr;
		motor(LeftBaseFront) = pwr;
		motor(RightBaseBack) = pwr;
		motor(RightBaseFront) = pwr;
		if (iLE > iDistance)
		{
			motor(LeftBaseBack) = 0;
			motor(LeftBaseFront) = 0;
			motor(RightBaseBack) = 0;
			motor(RightBaseFront) = 0;
			return;
		}
		if (pwr < 50)
		{
			wait1Msec(100);
		}
	}
}

void MoveForwardStraight(int iSpeed, int iLE, int iRE)
{
	if (iLE > iRE)
	{
		motor(LeftBaseBack) = iSpeed - 50;
		motor(LeftBaseFront) = iSpeed - 50;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
	else if (iRE > iLE)
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed - 50;
		motor(RightBaseFront) = iSpeed - 50;
	}
	else
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
}

void MoveBackwardStraight (int iSpeed, int iLE, int iRE)
{
	if (iLE > iRE)
	{
		motor(LeftBaseBack) = iSpeed + 50;
		motor(LeftBaseFront) = iSpeed + 50;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
	else if (iRE > iLE)
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed + 50;
		motor(RightBaseFront) = iSpeed + 50;
	}
	else
	{
		motor(LeftBaseBack) = iSpeed;
		motor(LeftBaseFront) = iSpeed;
		motor(RightBaseBack) = iSpeed;
		motor(RightBaseFront) = iSpeed;
	}
}

// bForward = true is forwards, false is backwards
//iDistance tells the encoder how far to go
void Move(bool bForward, int iDistance, int iSpeed)
{
	//Reset encoders
	nMotorEncoder(LeftBaseFront) = 0;
	nMotorEncoder(LeftBaseBack) = 0;
	nMotorEncoder(RightBaseFront) = 0;
	nMotorEncoder(RightBaseBack) = 0;
	// get the current value (will be zero to start cause we just reset it)
	int iLE = abs(nMotorEncoder(LeftBaseBack));
	int iRE = abs(nMotorEncoder(RightBaseBack));
	if (bForward)
	{
		// first ramp up slowly - then...
		RampUp(iSpeed, iDistance);
		// move forward until we reach encoder value given as iDistance
		while (iLE < iDistance)
		{
			MoveForwardStraight(iSpeed, iLE, iRE);
			iLE = abs(nMotorEncoder(LeftBaseBack));
			iRE = abs(nMotorEncoder(RightBaseBack));
		}
	}
	else
	{
		// we are moving backwards
		while (iLE < iDistance)
		{
			MoveBackwardStraight(-iSpeed, iLE, iRE);
			iLE = abs(nMotorEncoder(LeftBaseBack));
			iRE = abs(nMotorEncoder(RightBaseBack));
		}
	}
	motor(LeftBaseBack) = 0;
	motor(LeftBaseFront) = 0;
	motor(RightBaseBack) = 0;
	motor(RightBaseFront) = 0;
}

void MoveFullSpeed(bool bForwards, int iPosition)
{
	ClearEncoders();
	while (abs(nMotorEncoder(RightBaseBack)) < iPosition)
	{
		if (bForwards)
		{
			motor[RightBaseFront] = 127;
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront] = 127;
			motor[LeftBaseBack] = 127;
		}
		else
		{
			motor[RightBaseFront] = -127;
			motor[RightBaseBack] = -127;
			motor[LeftBaseFront] = -127;
			motor[LeftBaseBack] = -127;
		}

	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void Arms(bool bUp, int iPosition, int iMotorPower)
{
	//Move arms to trough, floor, or high
	// are we moving up or down
	if (bUp)
	{
		// we are moving up
		while (SensorValue[ArmPot] < iPosition)
		{
			motor(RightArm) = iMotorPower;
			motor(LeftArm) = iMotorPower;
		}
		// no movement - HOLD
		motor(RightArm) = ArmsMotorHold;
		motor(LeftArm) = ArmsMotorHold;
	}
	else if (!bUp)
	{
		// we are moving down
		while (SensorValue[ArmPot] > iPosition)
		{
			motor(RightArm) = -iMotorPower;
			motor(LeftArm) = -iMotorPower;
		}
		// no movement - HOLD
		motor(RightArm) = 0;
		motor(LeftArm) = 0;
	}
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//	Score the five stack striaght on...
// 	JAWS OPEN!!!!! Back of tile!!! Angled right towards the five stack!!!
//
////////////////////////////////////////////////////////////////////////////////////////
void ScoreFive()
{
	// flipper down to pick up five stack (opens jaws)
	flippergodown();

	//drive forward to five stack
	MoveFullSpeed(true, 1100); //1180

	// pick up five stack - raise flipper (close jaws once flipper is at 45 degree angle)
	flipperup();

	// raise arms up to trough
	Arms(true, ArmsTrough, 127);

	// drive forward
	MoveFullSpeed(true, 145);

	// score five stack
	flippertotrough();

	// drive back to starting tile
	MoveFullSpeed(false, 1200);
}

void ScoreHigh()
{
	// arms go from trough to high goal position
	Arms(true, ArmsHigh, 127);

	// flipper moves to straight up and down position
	flipperhigh();

	motor[LeftArm]=20;
	motor[RightArm]=20;

	// press button once bags are loaded on top of jaws
	WaitForLCD();

	// STRAIGHT SHOT TO HIGH GOAL
	MoveFullSpeed(true, 1310);
	wait1Msec(300);

	// flipper goes down to high goal and rest on high goal - jaws open to push bags to back of high goal
	flipperscorehigh();
	wait1Msec(600);
	// pullback slowly (jaw will be pushing outwards)
	Move(false, 600, SpeedSlow); //500
	motor(TheJaw) = 0;
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
	MoveFullSpeed(false, 1000);

	// turn robot towards opposite starting tile
	WaitForLCD();

	MoveFullSpeed(true, 1800);
	motor[FlipperMotorLeft]=(127);
	motor[FlipperMotorRight]=(127);
	wait1Msec(1200);
	motor[FlipperMotorLeft]=(0);
	motor[FlipperMotorRight]=(0);

	// at the same time: lower the arms and pull back towards opposite starting tile
	while (SensorValue[ArmPot] > ArmsFloor)
	{
		motor(RightArm) = -127;
		motor(LeftArm) = -127;
	}
	// everything off
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
	motor(RightArm) = 0;
	motor(LeftArm) = 0;
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////

void pre_auton()
{
	// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
	// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
	bStopTasksBetweenModes = true;

	//Clear LCD
	clearLCDLine(0);
	clearLCDLine(1);

	//Completely clear out any previous sensor readings by setting the port to "sensorNone"
	SensorType[in1] = sensorNone;
	wait1Msec(1000);
	//Reconfigure Analog Port 8 as a Gyro sensor and allow time for ROBOTC to calibrate it
	SensorType[in1] = sensorGyro;
	wait1Msec(2000);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{
	// Andgle towrads five stack and score it into trough without turning - straight shot
	ScoreFive();

	// Score High raises the arms and flipper into position without waiting for user
	// then waits for user to load stack onto jaws - press LCD button and the robot moves striaght, then
	// turns 90 degrees towards high goal - slowly - drops the stack onto the high goal post and backs up
	// towards the opposite starting tile and brings arms and flipper to floor position
	ScoreHigh();

	// angle the robot towards next five stack and press the lcd button
	WaitForLCD();

	// same as step one just on different side of field - stright shot
	ScoreFive();

}


/////////////////////////////////////////////////////////////////////////////////////////
//                                 User Control Task
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	while (true)
	{
		/*
		motor[RightArm] = -(vexRT(Ch2)/2);
		motor[LeftArm] = -(vexRT(Ch2)/2);
		motor[FlipperMotorRight] = (vexRT(Ch3)/2);
		motor[FlipperMotorLeft] = (vexRT(Ch3)/2);
		*/

		motor(LeftBaseFront) = vexRT(Ch3);
		motor(LeftBaseBack) = vexRT(Ch3);
		motor(RightBaseFront) = vexRT(Ch2);
		motor(RightBaseBack) = vexRT(Ch2);
	}
}
